/**
	@file mm.c
	@author Andrew D. Zonenberg
	@brief matrix multiplication test
 */
#include <achd-soc.h>
#include <stdarg.h>

void printf(const char* format, ...)	__attribute__ ((section(".startup")));
void print_char(char ch)				__attribute__ ((section(".startup")));
char* itoa(int n, char* s)				__attribute__ ((section(".startup")));
char* reverse(char* s)					__attribute__ ((section(".startup")));
int strlen(const char* str)				__attribute__ ((section(".startup")));

void cp0_timer_init();
int cp0_timer_value();

#define MAX_DIM 16

void main(void)
{
	//80 MHz / 115200 baud
	U1BRG = 695;
	
	//Speed up significantly for simulation
	//U1BRG = 24;
	
	//Turn on the LEDs
	LEDBANKA = 0x55;

	printf("Matrix multiplication test\r\n");
	
	int matrix_1[MAX_DIM][MAX_DIM];
	int matrix_2[MAX_DIM][MAX_DIM];
	int matrix_out[MAX_DIM][MAX_DIM];

	//init matrices
	for(int y=0; y<MAX_DIM; y++)
	{
		for(int x=0; x<MAX_DIM; x++)
		{
			 matrix_1[y][x] = y*3 + x;		//random location-dependent values
			 matrix_2[y][x] = y*7 + x*2;
		}
	}
	
 	cp0_timer_init();
	unsigned int start = cp0_timer_value();
	
	//multiply them
	for(int y=0; y<MAX_DIM; y++)
	{
		for(int x=0; x<MAX_DIM; x++)
		{
			int temp = 0;
			for(int k=0; k<MAX_DIM; k++)
				temp += matrix_1[y][k] * matrix_2[k][x];
			matrix_out[y][x] = temp;
		}
	}
		
	unsigned int delta = cp0_timer_value() - start;
	
	//and print
	int checksum = 0;
	for(int y=0; y<MAX_DIM; y++)
	{
		for(int x=0; x<MAX_DIM; x++)
		{
			printf("%d ", matrix_out[y][x]);
			checksum += matrix_out[y][x];
		}
		printf("\r\n");
	}
		
	printf("Checksum: %x (should be 87dc00)\r\n", checksum);
	int clocks = delta*2;
	int us = clocks / 80;	//80 MHz / 80 = 1 MHz
	printf("Elapsed time: %d clock cycles (%d us)\r\n", clocks, us);
	
	while(1);
}

/**
	@brief Very basic printf function
 */
void printf(const char* format, ...)
{
	va_list list;
	va_start(list, format);
	
	static const char hex[] = "0123456789abcdef";
	static const char Hex[] = "0123456789ABCDEF";
	char numbuf[12];	//= strlen("-2147483648") + room for null
						//largest possible string representation of a 32 bit value
	
	while(*format)
	{
		if(*format == '%')
		{
			//Get the format code
			//Don't support length specifiers for now
			format++;
			switch(*format)
			{
			case 's':
				{
					//It's a string
					const char* s = va_arg(list, const char*);
					while(*s)
					{
						print_char(*s);
						s++;
					}
				}
				break;
				
			case 'd':
				{
					//convert to an int
					itoa(va_arg(list, int), numbuf);
					for(int i=0; i<12; i++)
					{
						if(!numbuf[i])
							break;
						print_char(numbuf[i]);
					}
				}
				break;
				
			case 'x':
			case 'X':
				{
					unsigned int d = va_arg(list, unsigned int);
					int bFirst = 1;
					for(int j=0; j<8; j++)
					{
						//Grab the next 4 bits
						unsigned int x = d >> 28;
						
						//Print it
						char ch = hex[x];
						if(*format == 'X')					//capitalize
							ch = Hex[x];
							
						//Skip leading zeros unless we are padding
						//but print a single 0 if it's zero
						if( (ch == '0') && bFirst && (j != 7) )
						{
						}
						else
						{
							print_char(ch);
							bFirst = 0;
						}	
						
						//Shift off what we just printed
						d <<= 4;
					}	
				}
				break;
			default:
				//unrecognized character, ignore it
				break;
			}
		}
		
		else
			print_char(*format);
		
		format++;
	}	
	
	va_end(list);
}

/**
	@brief Prints a single character
 */
void print_char(char ch)
{
	if(U1STATbits.TXFULL)
	{
		while(!U1STATbits.TXEMPTY)
		{}
	}

	//Send the byte
	U1TXBUF = ch;
}

/*
	@brief Converts a number to a string
 */
char* itoa(int n, char* s)
{
	int i, sign;
	
	if ((sign = n) < 0)  /* record sign */
		n = -n;          /* make n positive */
	i = 0;
	do
	{       /* generate digits in reverse order */
		s[i++] = n % 10 + '0';   /* get next digit */
	} while ((n /= 10) > 0);     /* delete it */
	
	if (sign < 0)
		s[i++] = '-';
	s[i] = '\0';
	
	return reverse(s);
}	

/**
	@brief Reverses a string in-place (K&R implementation)
	
	@param s Input
	@return s after reversing
 */
char* reverse(char* s)
{
	int i, j;
	char c;
	
	for (i = 0, j = strlen(s)-1; i<j; i++, j--)
	{
		c = s[i];
		s[i] = s[j];
		s[j] = c;
	}
	return s;
}

/**
	@brief Returns the length of a null terminated string
 */
int strlen(const char* str)
{
	int i=0;
	while(str[i] != 0)
		i++;
	return i;
}
